/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "Polynomial.h"

#include "Utils.h"

#include <algorithm>
#include <numeric>
#include <functional>
#include <iostream>
#include <deque>

#include <cmath>

namespace RafMat
{
namespace SimEngine
{

using namespace std;

namespace {

    //
    // Definicje funktorów
    //

    // Odwrócona negacja
    template <class _Ty> struct rev_minus : public binary_function<_Ty, _Ty, _Ty>
    {
        // Definicja funktora
        _Ty operator() (const _Ty& _Left, const _Ty& _Right) const
        {
            // Oblicz odwróconą różnicę (od prawej odejmij lewą).
            return (_Right - _Left);
        }
    };

    // Akumulacja wartości bezwzględnych
    template <class _Ty> struct accum_abs : public binary_function<_Ty, _Ty, _Ty>
    {
        // Definicja funktora
        _Ty operator() (const _Ty& _Left, const _Ty& _Right) const
        {
            return _Left  + abs(_Right);
        }
    };

}

const double Polynomial::COMPARISON_TOLERANCE = 1e-15;
const double Polynomial::TRIM_TOLERANCE = 1e-30;

/**
    Wykonuje mnożenie dwóch wielomianów.

    Mnożenie odpowiada splotowi współczynników.

    \param [in] polyLhs lewa strona mnożenia
    \param [in] polyRhs prawa strona mnożenia
    \param [out] out Wynik mnożenia zwracany przez referencję
*/
void Polynomial::multiply(const Polynomial &polyLhs, const Polynomial &polyRhs, Polynomial &out)
{
    const PolyType &lhs = polyLhs.m_coeffs;
    const PolyType &rhs = polyRhs.m_coeffs;
    PolyType &r = out.m_coeffs;

    r.resize(lhs.size() + rhs.size() - 1);

    for (unsigned int i = 0; i < lhs.size(); i++)
    {
        for (unsigned int j = 0; j < rhs.size(); j++)
        {
            r[i + j] += lhs[i] * rhs[j];
        }
    }
}

/**
    Wykonuje dzielenie dwóch wielomianów.

    Funkcja zwraca tylko iloraz, reszta z dzielenia, o ile jest niezerowa, jest ignorowana.

    \param [in] polyLhs lewa strona dzielenia (dzielna)
    \param [in] polyRhs prawa strona dzielenia (dzielnik)
    \param [out] out Wynik dzielenia zwracany przez referencję
*/
void Polynomial::divide(const Polynomial &polyLhs, const Polynomial &polyRhs,
                   Polynomial &out)
{
    typedef deque<double> SampleMemory;

    // Dzielenie metodą odpowiedzi impulsowej (dekonwolucja)
    // Idea polega na filtracji impulsu kroneckera filtrem polyLhs / polyRhs

    // Rhs powinien być moniczny:
    Polynomial lhs = polyLhs,
            rhs = polyRhs;

    int dL = lhs.degree(),
        dR = rhs.degree(),
        dO = dL - dR;           // Stopień ilorazu

    // Zwróć zero jeśli dzielnik wyższego stopnia niż dzielna
    if(dO < 0)
    {
        out = 0;
        return;
    }

    lhs /= rhs[0];
    rhs /= rhs[0];

    // Pamięci dla próbek L i R (symulacja obiektu L/R)
    SampleMemory rmem(dR);

    for(int i = 0; i < dO+1; i++)
    {
        double tmpVal = 0;

        PolyType::iterator
                rpoly_it = ++rhs.begin(),       // od r1
                rpoly_last_it = rhs.end();      // do rnR

        SampleMemory::iterator
                rmem_it = rmem.begin();

        // Obliczenie -r1*y(i-1) .. -rdR*y(i-dR)
        tmpVal = inner_product(rpoly_it, rpoly_last_it, rmem_it, double(0), minus<double>(), multiplies<double>());

        // Obliczenie l0*u(i) + ... + ldL*u(i-dL)
        // u - zawiera impuls kroneckera, więc zamiast inner_product
        // można to znacznie uprościć:
        if(i <= dL)
        {
            tmpVal += lhs[i];
        }

        // Wstawienie wyniku do out:
        out.setCoeff(i, tmpVal);

        rmem.push_front(tmpVal);
        rmem.pop_back();
    }
}

/**
    Wykonuje dzielenie dwóch wielomianów.

    Funkcja zwraca wynik dzielenia i resztę dzieenia.

    \param [in] polyLhs lewa strona dzielenia (dzielna)
    \param [in] polyRhs prawa strona dzielenia (dzielnik)
    \param [out] quotient Wynik dzielenia zwracany przez referencję
    \param [out] reminder Reszta dzielenia zwracana przez referencję
*/
void Polynomial::divide(const Polynomial &polyLhs, const Polynomial &polyRhs,
                   Polynomial &quotient, Polynomial &reminder)
{
    Polynomial polyRhs0 = polyRhs.trim();  
    reminder = polyLhs.trim();

    if(polyRhs0.degree() == 0)
    {
        // Dzielenie przez stałą
        quotient = reminder / polyRhs0[0];
        reminder = 0;
        return;
    }

    quotient = 0;
    int dg = reminder.degree() - polyRhs0.degree();

    if(dg < 0)
        return;

    while(true)
    {
        // Dziel przez najwyższy współczynnik
        quotient.setCoeff(dg, reminder.high() / polyRhs0.high());

        // Wymnóż i odejmij
        reminder -= quotient.getMonomial(dg) * polyRhs0;

        // Usuń ewentualne zera przy najwyższych potęgach
        reminder.trimSelf();

        // Warunek stopu
        if(!dg--)
            break;
    }
}

void Polynomial::gcd(const Polynomial &p, const Polynomial &q, Polynomial &g)
{
    (void) p;
    (void) q;
    (void) g;

    // Not implemented
}

Polynomial::Polynomial(double c)
{
    m_coeffs.push_back(c);
}

Polynomial::Polynomial(int degree, double initVal)
{
    m_coeffs.resize(degree + 1, initVal);
}

Polynomial::Polynomial(const Polynomial &c)
{
    // cout << "Wykonano kopie wielomianu." << endl;
    m_coeffs = c.m_coeffs;
}

Polynomial::Polynomial(const PolyType &coeffs)
{
    // Utwórz kopię współczynników.
    m_coeffs = coeffs;

    // Minimalny stopien to zero - wymaga przynajmniej jednego wspolczynnika
    if(m_coeffs.size() < 1)
        m_coeffs.push_back(1);
}

double Polynomial::norm() const
{
    return accumulate(m_coeffs.begin(), m_coeffs.end(), double(0), accum_abs<double>());
}

Polynomial Polynomial::trim() const
{
    Polynomial cpy(*this);

    while(abs(cpy.m_coeffs.back()) < TRIM_TOLERANCE && cpy.m_coeffs.size() > 1)
        cpy.m_coeffs.pop_back();

    return cpy;
}

void Polynomial::trimSelf()
{
    while(abs(m_coeffs.back()) < TRIM_TOLERANCE && m_coeffs.size() > 1)
        m_coeffs.pop_back();
}


/*
Polynomial Polynomial::trim(int tcount) const
{
    Polynomial cpy(*this);

    // Zmniejsz rozmiar o odpowiednią liczbę wsp. (nie może zejść poniżej size = 1).
    cpy.m_coeffs.resize(max(m_coeffs.size() - tcount, size_t(1)));

    return cpy;
}*/

int Polynomial::dummyZeros() const
{
    int dummyCounter = 0;
    PolyType::const_reverse_iterator it = m_coeffs.rbegin();

    while(abs(*it) < TRIM_TOLERANCE && it != m_coeffs.rend())
        dummyCounter++;

    return dummyCounter;
}

Polynomial Polynomial::reverse() const
{
    return Polynomial(m_coeffs.rbegin(), m_coeffs.rend(), false);
}

void Polynomial::limitCoeffs(double limitVal)
{
    for(PolyType::iterator it = m_coeffs.begin(); it != m_coeffs.end(); ++it)
    {
        *it = Utils::limitValue(*it, -limitVal, limitVal);
    }
}

Polynomial& Polynomial::operator += (const Polynomial &polyRhs)
{
    const PolyType &rhs = polyRhs.m_coeffs;
    m_coeffs.resize(max(m_coeffs.size(), rhs.size()));

    transform(rhs.begin(), rhs.end(), m_coeffs.begin(), m_coeffs.begin(), plus<double>());

    return *this;
}

/**
    Odejmuje od bierzącej instancji inny wielomian. Umożliwia zapis typu:

    \code

 Polynomian a, b;
    a -= b + 5;
    \endcode

    \param [in] polyRhs prawa strona przypisania
    \return Referencja do bierzącego obiektu (zmienionego).
*/
Polynomial& Polynomial::operator -= (const Polynomial &polyRhs)
{
    const PolyType &rhs = polyRhs.m_coeffs;
    m_coeffs.resize(max(m_coeffs.size(), rhs.size()));

    transform(rhs.begin(), rhs.end(), m_coeffs.begin(), m_coeffs.begin(), rev_minus<double>());

    return *this;
}

Polynomial& Polynomial::operator *= (double cRhs)
{
    PolyType::iterator it;

    // TODO: Dla cRhs == 0, powinno być skrócenie wektora m_coeffs.

    for(it = m_coeffs.begin(); it != m_coeffs.end(); ++it)
    {
        *it *= cRhs;
    }

    return *this;
}

/** Przypisanie z mnożeniem. */
Polynomial& Polynomial::operator *= (const Polynomial &polyRhs)
{
    Polynomial out;
    Polynomial::multiply(*this, polyRhs, out);

    this->m_coeffs = out.m_coeffs;
    return *this;
}

Polynomial& Polynomial::operator /= (double cRhs)
{
    PolyType::iterator it;

    for(it = m_coeffs.begin(); it != m_coeffs.end(); ++it)
    {
        *it /= cRhs;
    }

    return *this;
}

Polynomial& Polynomial::operator /= (const Polynomial &polyRhs)
{
    Polynomial quot;
    Polynomial::divide(*this, polyRhs, quot);
    return *this = quot;
}


double Polynomial::evaluate(double arg) const
{
    double accumulator = 0;

    PolyType::const_reverse_iterator it;
    for(it = m_coeffs.rbegin(); it < m_coeffs.rend(); ++it)
    {
        accumulator = (accumulator * arg) + (*it);
    }

    return accumulator;
}


Polynomial operator + (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    return Polynomial(polyLhs) += polyRhs;
}

Polynomial operator - (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    return Polynomial(polyLhs) -= polyRhs;
}

Polynomial Polynomial::operator - () const
{
    Polynomial p(*this);
    PolyType &ref = p.m_coeffs;
    transform(ref.begin(), ref.end(), ref.begin(), std::negate<double>());
    return p;
}

Polynomial operator * (double c, const Polynomial &polyRhs)
{
    Polynomial p(polyRhs);
    Polynomial::PolyType &ref = p.m_coeffs;
    transform(ref.begin(), ref.end(), ref.begin(), std::bind2nd(std::multiplies<double>(), c));
    return p;
}

Polynomial operator * (const Polynomial &polyLhs, double c)
{
    Polynomial p(polyLhs);
    Polynomial::PolyType &ref = p.m_coeffs;
    transform(ref.begin(), ref.end(), ref.begin(), std::bind2nd(std::multiplies<double>(), c));
    return p;
}

Polynomial operator * (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    Polynomial p(polyLhs);
    return p *=  polyRhs;
}

Polynomial operator / (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    Polynomial p;
    Polynomial::divide(polyLhs, polyRhs, p);
    return p;
}

Polynomial operator / (const Polynomial &polyLhs, double c)
{
    Polynomial p(polyLhs);
    Polynomial::PolyType &ref = p.m_coeffs;
    transform(ref.begin(), ref.end(), ref.begin(), std::bind2nd(std::multiplies<double>(), 1.0/c));
    return p;
}

Polynomial operator << (const Polynomial &poly, unsigned int i)
{
    Polynomial::PolyType c = poly.m_coeffs;

    if(c.size()-1 < i)
        return Polynomial();

    rotate(c.begin(), c.begin() + i, c.end());
    c.resize(c.size() - i);

    return Polynomial(c);
}

Polynomial operator >> (const Polynomial &poly, unsigned int i)
{
    Polynomial::PolyType c = poly.m_coeffs;

    c.resize(c.size() + i);
    rotate(c.begin(), c.end()-i, c.end());

    return Polynomial(c);
}

bool operator == (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    return (polyRhs - polyLhs).norm() < Polynomial::COMPARISON_TOLERANCE;
}

bool operator != (const Polynomial &polyLhs, const Polynomial &polyRhs)
{
    return !(polyLhs == polyRhs);
}

std::ostream& operator << (std::ostream& os, const Polynomial &poly)
{
    os << "[ ";

    for(Polynomial::PolyType::const_iterator it = poly.m_coeffs.begin(); it != poly.m_coeffs.end(); ++it)
    {
        os << *it << ' ';
    }

    os << ']';
    return os;
}

}   // SimEngine namespace
}   // RafMat namespace

